﻿using lctdevice;
using Microsoft.Extensions.Logging;
using RotovapPlatform.Infrastructure.Equipment.Communication.Ethercat;
using RotovapPlatform.Domain.Equipment.Interfaces.Base;
using RotovapPlatform.Domain.Equipment.Device.Motors.Dto;
using RotovapPlatform.Domain.Equipment.Interfaces.Communication;
using RotovapPlatform.Domain.Equipment.DeviceParameters;

namespace RotovapPlatform.Infrastructure.Equipment.Devices.EtherCATMotor
{
	/// <summary>
	/// EtherCAT电机驱动
	/// </summary>
	public class EtherCATMotorDrive : IMotorBase
	{
		private readonly ILogger<EtherCATMotorDrive> _logger;
		private readonly IEtherCATManager _manager;
		private MotorParameters _motorParams;

		public string EquipmentId { get; }
		public string Name { get; }
		public short AxisNo { get; }

		/// <summary>
		/// 构造函数
		/// </summary>
		/// <param name="equipmentId">设备ID</param>
		/// <param name="name">设备名称</param>
		/// <param name="logger">日志记录器</param>
		/// <param name="manager">EtherCAT管理器</param>
		/// <param name="axisNo">轴号</param>
		public EtherCATMotorDrive(
			string equipmentId,
			string name,
			ILogger<EtherCATMotorDrive> logger,
			IEtherCATManager manager,
			short axisNo)
		{
			EquipmentId = equipmentId;
			Name = name;
			_logger = logger;
			_manager = manager;
			AxisNo = axisNo;

			// 初始化默认参数
			_motorParams = new MotorParameters();
		}

		/// <summary>
		/// 设置电机参数
		/// </summary>
		/// <param name="parameters">电机参数</param>
		public void SetMotorParameters(MotorParameters parameters)
		{
			if (parameters != null)
			{
				_motorParams = parameters;
				_logger.LogInformation($"电机 {EquipmentId} 参数已设置: PulsesPerRevolution={parameters.PulsesPerRevolution}, GearRatio={parameters.GearRatio}");
			}
		}

		/// <summary>
		/// 获取电机参数
		/// </summary>
		/// <returns>电机参数</returns>
		public MotorParameters GetMotorParameters()
		{
			return _motorParams;
		}

		/// <summary>
		/// 设置电机运动参数
		/// </summary>
		/// <param name="position">目标位置</param>
		/// <param name="speed">速度</param>
		/// <param name="acceleration">加速度</param>
		/// <param name="deceleration">减速度</param>
		/// <returns>是否成功</returns>
		public async Task<bool> SetMotionParametersAsync(double position, double speed, double acceleration, double deceleration)
		{
			try
			{
				// 这里需要根据具体的控制器接口设置电机运动参数
				// 例如，设置目标位置、速度、加速度和减速度
				_logger.LogInformation($"设置电机 {EquipmentId} 运动参数: 位置={position}, 速度={speed}, 加速度={acceleration}, 减速度={deceleration}");

				// TODO: 添加实际的参数设置代码，如果控制器支持的话

				return true;
			}
			catch (Exception ex)
			{
				_logger.LogError(ex, $"设置电机 {EquipmentId} 运动参数失败");
				return false;
			}
		}

		public async Task<bool> EnableAsync()
		{
			try
			{
				short ret = ecat_motion.M_Servo_On(AxisNo, _manager.CardId);
				if (ret != 0)
				{
					_logger.LogError("使能伺服失败，轴号：{AxisId}，错误码：{ErrorCode}", AxisNo, ret);
					return false;
				}
				return true;
			}
			catch (Exception ex)
			{
				_logger.LogError(ex, "使能伺服异常，轴号：{AxisId}", AxisNo);
				return false;
			}
		}

		public async Task<bool> DisableAsync()
		{
			try
			{
				short ret = ecat_motion.M_Servo_Off(AxisNo, _manager.CardId);
				if (ret != 0)
				{
					_logger.LogError("去使能伺服失败，轴号：{AxisId}，错误码：{ErrorCode}", AxisNo, ret);
					return false;
				}
				return true;
			}
			catch (Exception ex)
			{
				_logger.LogError(ex, "去使能伺服异常，轴号：{AxisId}", AxisNo);
				return false;
			}
		}

		public async Task<bool> HomeAsync()
		{
			try
			{
				// 1. 设置回零模式
				short ret = ecat_motion.M_SetHomingMode(AxisNo, 6, _manager.CardId); // 6对应驱动器的回零工作模式(Homing)
				await Task.Delay(50); // 此延时有必要
				if (ret != 0)
				{
					_logger.LogError("设置回零模式失败，轴号：{AxisId}，错误码：{ErrorCode}", AxisNo, ret);
					return false;
				}

				// 2. 启动回零
				ret = ecat_motion.M_HomingStart(AxisNo, _manager.CardId);
				if (ret != 0)
				{
					_logger.LogError("启动回零失败，轴号：{AxisId}，错误码：{ErrorCode}", AxisNo, ret);
					return false;
				}

				_logger.LogInformation("轴 {AxisId} 开始回零", AxisNo);

				// 3. 等待回零完成
				return await Task.Run(async () =>
				{
					try
					{
						int currentAxisStatus;
						// 等待运动停止
						do
						{
							await Task.Delay(100);
							ret = ecat_motion.M_GetSts(AxisNo, out currentAxisStatus, 1, _manager.CardId);
							if (ret != 0)
							{
								_logger.LogError("获取轴状态失败，轴号：{AxisId}，错误码：{ErrorCode}", AxisNo, ret);
								return false;
							}

							// 检查是否有报警
							if ((currentAxisStatus & 0x0002) != 0)
							{
								_logger.LogError("轴 {AxisId} 回零过程中出现报警", AxisNo);
								return false;
							}

							// 检查是否到达限位
							if ((currentAxisStatus & 0x0060) != 0)
							{
								_logger.LogError("轴 {AxisId} 回零过程中触发限位", AxisNo);
								return false;
							}

							// 检查回零错误
							if ((currentAxisStatus & 0x10000) != 0)
							{
								_logger.LogError("轴 {AxisId} 回零过程中出现回零错误", AxisNo);
								return false;
							}

						} while ((currentAxisStatus & 0x400) == 0x400); // 等待运动完成信号

						//// 4. 切换回位置模式
						//short mode = 0;
						//short count = 0;
						//do
						//{
						//	ret = ecat_motion.M_SetHomingMode(AxisNo, 8, _manager.CardId);
						//	count++;
						//	await Task.Delay(50);
						//	ret = ecat_motion.M_EcatGetOperationMode(AxisNo, ref mode, _manager.CardId);
						//	if ((mode == 8) || (count > 10))
						//		break;
						//} while (true);

						//if (count > 10)
						//{
						//	_logger.LogError("轴 {AxisId} 切换位置模式失败", AxisNo);
						//	return false;
						//}

						// 5. 清除状态
						ecat_motion.M_ClrSts(AxisNo, 1, 0);

						// 6. 检查回零结果
						double cmdPos = 0;
						ret = ecat_motion.M_GetCmd(AxisNo, out cmdPos, 1, _manager.CardId);
						if (ret != 0)
						{
							_logger.LogError("获取规划位置失败，轴号：{AxisId}，错误码：{ErrorCode}", AxisNo, ret);
							return false;
						}

						// 获取回零参数
						short homeMode = 0;      // 回零模式
						int homeOffset = 0;      // 回零偏移
						uint homeSpeed1 = 0;     // 第一回零速度
						uint homeSpeed2 = 0;     // 第二回零速度
						uint homeAcc = 0;        // 回零加速度
						ushort probeFunction = 0;// 预留，默认为 0
						ret = ecat_motion.M_GetHomingPrm(AxisNo, ref homeMode, ref homeOffset,
							ref homeSpeed1, ref homeSpeed2, ref homeAcc, ref probeFunction, _manager.CardId);
						if (ret != 0)
						{
							_logger.LogError("获取回零参数失败，轴号：{AxisId}，错误码：{ErrorCode}", AxisNo, ret);
							return false;
						}

						// 检查最终位置是否在原点附近
						if (Math.Abs(cmdPos - homeOffset) < 50) // 50是经验值，表示50个脉冲
						{
							_logger.LogInformation("轴 {AxisId} 回零成功", AxisNo);
							return true;
						}
						else
						{
							_logger.LogWarning("轴 {AxisId} 回零完成，但位置偏差过大，当前位置：{Position}，目标位置：{Target}",
								AxisNo, cmdPos, homeOffset);
							return false;
						}
					}
					catch (Exception ex)
					{
						_logger.LogError(ex, "轴 {AxisId} 等待回零完成时发生异常", AxisNo);
						return false;
					}
				});
			}
			catch (Exception ex)
			{
				_logger.LogError(ex, "轴 {AxisId} 执行回零时发生异常", AxisNo);
				return false;
			}
		}

		public async Task<MotorStatus> GetStatusAsync()
		{
			var status = new MotorStatus();
			try
			{
				// 获取轴状态字
				int axisStatus = _manager.GetAxisStatus(AxisNo);
				if (axisStatus == -1)
				{
					status.Offline = true;
					return status;
				}

				// 解析状态位
				status.ServoAlarm = (axisStatus & 0x0002) != 0;     // Bit1 ALM
				status.ForwardLimit = (axisStatus & 0x0020) != 0;   // Bit5 PEL
				status.ReverseLimit = (axisStatus & 0x0040) != 0;   // Bit6 MEL
				status.Enabled = (axisStatus & 0x0200) != 0;        // Bit9 SVON
				status.Running = (axisStatus & 0x0400) != 0;        // Bit10 MOVE
				status.InPosition = (axisStatus & 0x0800) != 0;     // Bit11 INP
				status.HomeError = (axisStatus & 0x10000) != 0;     // Bit16
				status.Homed = (axisStatus & 0x20000) != 0;         // Bit17
				status.Position_OK = (axisStatus & 0x40000) != 0;   // Bit18
				status.OriginSwitch = (axisStatus & 0x100000) != 0; // Bit20

				// 获取实际位置、速度、扭矩
				int pActualPos = _manager.GetAxisPosition(AxisNo);
				double pActualVel = _manager.GetAxisVelocity(AxisNo);
				int pActualTorque = _manager.GetAxisTorque(AxisNo);

				// 根据电机参数转换位置单位（脉冲->用户单位）
				if (_motorParams.PulsesPerRevolution > 0 && _motorParams.GearRatio > 0 && _motorParams.DisplacementPerRevolution > 0)
				{
					// 计算转换因子：每个脉冲代表多少用户单位
					// 公式：用户单位 = 脉冲数 / (每转脉冲数 * 减速比) * 每转位移量
					double conversionFactor = _motorParams.DisplacementPerRevolution / (_motorParams.PulsesPerRevolution * _motorParams.GearRatio);
					status.Actual_P = pActualPos * conversionFactor;
				}
				else
				{
					status.Actual_P = pActualPos;
				}

				status.Actual_V = pActualVel;
				status.Actual_T = pActualTorque;

				// 获取目标位置、速度
				double pTargetPos = _manager.GetAxisPlanPosition(AxisNo);
				double pTargetVel = _manager.GetAxisPlanVelocity(AxisNo);

				// 根据电机参数转换位置单位（脉冲->用户单位）
				if (_motorParams.PulsesPerRevolution > 0 && _motorParams.GearRatio > 0 && _motorParams.DisplacementPerRevolution > 0)
				{
					// 使用相同的转换因子计算目标位置
					double conversionFactor = _motorParams.DisplacementPerRevolution / (_motorParams.PulsesPerRevolution * _motorParams.GearRatio);
					status.Target_P = pTargetPos * conversionFactor;
				}
				else
				{
					status.Target_P = pTargetPos;
				}

				status.Target_V = pTargetVel;
			}
			catch (Exception ex)
			{
				_logger.LogError(ex, "获取电机状态异常，轴号：{AxisId}", AxisNo);
				status.Offline = true;
			}

			return status;
		}

		/// <summary>
		/// 执行绝对定位
		/// </summary>
		/// <param name="position">目标位置（用户单位）</param>
		/// <param name="speed">运行速度（rpm，转/分钟）</param>
		/// <param name="acceleration">加速度</param>
		/// <param name="deceleration">减速度</param>
		/// <returns>是否成功</returns>
		public async Task<bool> AbsoluteMoveAsync(double position, double speed, double acceleration, double deceleration)
		{
			try
			{
				// 转换位置（用户单位->脉冲）
				double targetPosition = position;
				double targetSpeed = speed;

				// 如果有电机参数，进行单位转换
				if (_motorParams.PulsesPerRevolution > 0 && _motorParams.GearRatio > 0 && _motorParams.DisplacementPerRevolution > 0)
				{
					// 计算转换因子：每个用户单位需要多少脉冲
					// 公式：脉冲数 = (位置 / 每转位移量) * 每转脉冲数 * 减速比
					double conversionFactor = _motorParams.PulsesPerRevolution * _motorParams.GearRatio / _motorParams.DisplacementPerRevolution;
					targetPosition = position * conversionFactor;

					// 速度单位转换：rpm(转/分钟) -> 脉冲/秒
					// 每分钟speed转 -> 每转PulsesPerRevolution个脉冲 -> 考虑减速比 -> 除以60转为每秒
					targetSpeed = speed * _motorParams.PulsesPerRevolution * _motorParams.GearRatio / 60.0;
				}

				// 记录参数
				_logger.LogInformation($"轴 {AxisNo} 开始绝对运动: 位置={position}({targetPosition}脉冲), 速度={speed}rpm({targetSpeed}脉冲/秒)");

				// 执行绝对运动
				short ret = ecat_motion.M_AbsMove(AxisNo, (int)targetPosition, targetSpeed, _manager.CardId);
				if (ret != 0)
				{
					_logger.LogError("绝对运动启动失败，轴号：{AxisId}，错误码：{ErrorCode}", AxisNo, ret);
					return false;
				}

				// 等待运动完成
				return await Task.Run(async () =>
				{
					try
					{
						int currentAxisStatus;
						do
						{
							await Task.Delay(100);
							ret = ecat_motion.M_GetSts(AxisNo, out currentAxisStatus, 1, _manager.CardId);
							if (ret != 0)
							{
								_logger.LogError("获取轴状态失败，轴号：{AxisId}，错误码：{ErrorCode}", AxisNo, ret);
								return false;
							}

							// 检查是否有报警
							if ((currentAxisStatus & 0x0002) != 0)
							{
								_logger.LogError("轴 {AxisId} 运动过程中出现报警", AxisNo);
								return false;
							}

							// 检查是否到达限位
							if ((currentAxisStatus & 0x0060) != 0)
							{
								_logger.LogError("轴 {AxisId} 运动过程中触发限位", AxisNo);
								return false;
							}

						} while ((currentAxisStatus & 0x400) == 0x400); // 等待运动完成信号

						// 等待到位信号
						int inPositionTimeout = 50; // 5秒超时
						while ((currentAxisStatus & 0x800) == 0 && inPositionTimeout > 0)
						{
							await Task.Delay(100);
							ret = ecat_motion.M_GetSts(AxisNo, out currentAxisStatus, 1, _manager.CardId);
							inPositionTimeout--;
						}

						bool success = (currentAxisStatus & 0x800) != 0;
						if (success)
						{
							_logger.LogInformation("轴 {AxisId} 绝对运动完成，已到达目标位置 {Position}", AxisNo, position);
						}
						else
						{
							_logger.LogWarning("轴 {AxisId} 绝对运动完成，但未检测到到位信号", AxisNo);
						}

						return success;
					}
					catch (Exception ex)
					{
						_logger.LogError(ex, "等待轴 {AxisId} 运动完成时发生异常", AxisNo);
						return false;
					}
				});
			}
			catch (Exception ex)
			{
				_logger.LogError(ex, "轴 {AxisId} 执行绝对运动时发生异常", AxisNo);
				return false;
			}
		}

		public async Task<bool> RelativeMoveAsync()
		{
			try
			{
				// 1. 执行相对运动
				short ret = ecat_motion.M_RelMove(AxisNo, 10000000, 1000000, _manager.CardId);
				if (ret != 0)
				{
					_logger.LogError("相对运动启动失败，轴号：{AxisId}，错误码：{ErrorCode}", AxisNo, ret);
					return false;
				}

				_logger.LogInformation("轴 {AxisId} 开始相对运动", AxisNo);

				// 2. 等待运动完成
				return await Task.Run(async () =>
				{
					try
					{
						int currentAxisStatus;
						do
						{
							await Task.Delay(100);
							ret = ecat_motion.M_GetSts(AxisNo, out currentAxisStatus, 1, _manager.CardId);
							if (ret != 0)
							{
								_logger.LogError("获取轴状态失败，轴号：{AxisId}，错误码：{ErrorCode}", AxisNo, ret);
								return false;
							}

							// 检查是否有报警
							if ((currentAxisStatus & 0x0002) != 0)
							{
								_logger.LogError("轴 {AxisId} 运动过程中出现报警", AxisNo);
								return false;
							}

							// 检查是否到达限位
							if ((currentAxisStatus & 0x0060) != 0)
							{
								_logger.LogError("轴 {AxisId} 运动过程中触发限位", AxisNo);
								return false;
							}

						} while ((currentAxisStatus & 0x400) == 0x400); // 等待运动完成信号

						// 等待到位信号
						int inPositionTimeout = 50; // 5秒超时
						while ((currentAxisStatus & 0x800) == 0 && inPositionTimeout > 0)
						{
							await Task.Delay(100);
							ret = ecat_motion.M_GetSts(AxisNo, out currentAxisStatus, 1, _manager.CardId);
							inPositionTimeout--;
						}

						bool success = (currentAxisStatus & 0x800) != 0;
						if (success)
						{
							_logger.LogInformation("轴 {AxisId} 相对运动完成", AxisNo);
						}
						else
						{
							_logger.LogWarning("轴 {AxisId} 相对运动完成，但未检测到到位信号", AxisNo);
						}

						return success;
					}
					catch (Exception ex)
					{
						_logger.LogError(ex, "等待轴 {AxisId} 运动完成时发生异常", AxisNo);
						return false;
					}
				});
			}
			catch (Exception ex)
			{
				_logger.LogError(ex, "轴 {AxisId} 执行相对运动时发生异常", AxisNo);
				return false;
			}
		}

		Task<bool> IMotorBase.SpeedRunAsync()
		{
			throw new NotImplementedException();
		}

		public async Task<bool> StopJogAsync()
		{
			try
			{
				short ret = ecat_motion.M_StopSingleAxis(AxisNo, 1, _manager.CardId);
				_logger.LogInformation("轴 {AxisId} 停止Jog运动", AxisNo);
				return true;
			}
			catch (Exception ex)
			{
				_logger.LogError(ex, "停止Jog异常，轴号：{AxisId}", AxisNo);
				return false;
			}
		}

		public async Task<bool> JogPlusAsync(double jogSpeed) //rpm
		{
			try
			{
				//rpm(转/分钟) -> 脉冲/秒
				double jogSpeed_pulse;

				if (_motorParams.PulsesPerRevolution > 0 && _motorParams.GearRatio > 0)
				{
					// 直接计算：rpm -> 脉冲/秒
					// 每分钟jogSpeed转 -> 每转PulsesPerRevolution个脉冲 -> 考虑减速比 -> 除以60转为每秒
					jogSpeed_pulse = jogSpeed * _motorParams.PulsesPerRevolution * _motorParams.GearRatio / 60.0;
				}
				else
				{
					// 兼容旧方式：rpm->脉冲/s
					jogSpeed_pulse = jogSpeed * _motorParams.PulsesPerRevolution / 60.0;
				}

				short ret = ecat_motion.M_Jog(AxisNo, jogSpeed_pulse, _manager.CardId);
				_logger.LogInformation("轴 {AxisId} 开始Jog+运动，速度={Speed}rpm ({PulseSpeed}脉冲/秒)", AxisNo, jogSpeed, jogSpeed_pulse);
				return true;
			}
			catch (Exception ex)
			{
				_logger.LogError(ex, "Jog+异常，轴号：{AxisId}", AxisNo);
				return false;
			}
		}

		public async Task<bool> JogMinusAsync(double jogSpeed) //rpm
		{
			try
			{
				//rpm(转/分钟) -> 脉冲/秒
				double jogSpeed_pulse;

				if (_motorParams.PulsesPerRevolution > 0 && _motorParams.GearRatio > 0)
				{
					// 直接计算：rpm -> 脉冲/秒
					// 每分钟jogSpeed转 -> 每转PulsesPerRevolution个脉冲 -> 考虑减速比 -> 除以60转为每秒
					jogSpeed_pulse = jogSpeed * _motorParams.PulsesPerRevolution * _motorParams.GearRatio / 60.0;
				}
				else
				{
					// 兼容旧方式：rpm->脉冲/s
					jogSpeed_pulse = jogSpeed * _motorParams.PulsesPerRevolution / 60.0;
				}

				short ret = ecat_motion.M_Jog(AxisNo, -jogSpeed_pulse, _manager.CardId);
				_logger.LogInformation("轴 {AxisId} 开始Jog-运动，速度={Speed}rpm ({PulseSpeed}脉冲/秒)", AxisNo, jogSpeed, jogSpeed_pulse);
				return true;
			}
			catch (Exception ex)
			{
				_logger.LogError(ex, "Jog-异常，轴号：{AxisId}", AxisNo);
				return false;
			}
		}

		//设置扭模式下的最大限速uint  脉冲/s
		public async Task<bool> SetMaxSpeedAsync(uint maxSpeed)
		{
			short slaveNo = 0;
			short ret = ecat_motion.M_GetAxisSlaveIndex(AxisNo, ref slaveNo, _manager.CardId);
		     ret = ecat_motion.M_EcatSDOWrite(slaveNo, 0x607F, 0x00, maxSpeed, 4, _manager.CardId);
			_logger.LogInformation("设置扭矩模式下的最大限速", ret);
			return ret == 0;
		}

		//设置模式
		public async Task<bool> SetModeAsync(short targetMode)
		{
			//10:扭矩模式
			short ret1 = ecat_motion.M_EcatSetOperationMode(AxisNo, targetMode, _manager.CardId);
			await Task.Delay(200);
			short mode = 0;
			short ret2 = ecat_motion.M_EcatGetOperationMode(AxisNo, ref mode, _manager.CardId);

			if (ret1 == 0 && ret2 == 0 && mode == targetMode)
			{
				_logger.LogInformation("设置模式成功");
				return true;
			}
			else
			{
				_logger.LogError("设置模式失败");
				return false;
			}
		}

		//设置转矩千分比，转矩变化率
		public async Task<bool> SetTorqueAsync(short TargetTorque, double TorqueChangeRate)
		{
			short ret = ecat_motion.M_WriteTargetTorque(AxisNo, TargetTorque, TorqueChangeRate, _manager.CardId);
			_logger.LogInformation("设置转矩千分比，转矩变化率", ret);
			return ret == 0;
		}

		//读取当前扭矩
		public async Task<short> GetCurrentTorqueAsync()
		{
			short pTorque = 0;
			short ret = ecat_motion.M_ReadActualTorque(AxisNo, ref pTorque, 1, _manager.CardId);
			if (ret == 0)
				return pTorque;
			else
				return 0;
		}

		/// <summary>
		/// 复位轴
		/// </summary>
		/// <returns>操作是否成功</returns>
		public async Task<bool> ResetAxisAsync()
		{
			try
			{
				_logger.LogInformation("开始复位轴 {AxisId}", AxisNo);

				// 执行轴复位
				short ret = ecat_motion.M_ClrSts(AxisNo, 1, _manager.CardId);
				if (ret != 0)
				{
					_logger.LogError("复位轴失败，轴号：{AxisId}，错误码：{ErrorCode}", AxisNo, ret);
					return false;
				}

				_logger.LogInformation("轴 {AxisId} 复位成功", AxisNo);
				return true;
			}
			catch (Exception ex)
			{
				_logger.LogError(ex, "复位轴时发生异常，轴号：{AxisId}", AxisNo);
				return false;
			}
		}

	}
}